#include "TurntableController.hpp"

TurntableController::TurntableController():
	dragging(DRAG_NULL) {
	reset();
}

void TurntableController::reset() {
	elevation = 0.0;
	azimuth = 0.0;
	range = 10.0;
	lookAt = Eigen::Vector3d::Zero();

	dragging = DRAG_NULL;

	updateWorldToView();
}

void TurntableController::beginDragRotate(const Eigen::Vector2f& mousePt) {
	beginDrag(mousePt, DRAG_ROTATE);
}

void TurntableController::beginDragLookat(const Eigen::Vector2f& mousePt) {
	beginDrag(mousePt, DRAG_LOOKAT);
}

void TurntableController::beginDragLookatZ(const Eigen::Vector2f& mousePt) {
	beginDrag(mousePt, DRAG_LOOKAT_Z);
}

void TurntableController::beginDrag(const Eigen::Vector2f& mousePt, DragType type) {
	assert(dragging == DRAG_NULL);
	assert(type != DRAG_NULL);

	dragging = type;
	curPt = startPt = mousePt;
}

void TurntableController::updateDrag(const Eigen::Vector2f& mousePt) {
	if (dragging != DRAG_NULL) {
		curPt = mousePt;
		const Eigen::Vector2f delta = curPt - startPt;

		if (dragging == DRAG_ROTATE) {
			double az = azimuth;
			double el = elevation;
			interpretDragRotate(delta, az, el);
			updateWorldToView(az, el, range, lookAt);
		} else if (dragging == DRAG_LOOKAT) {
			Eigen::Vector3d lk = lookAt;
			interpretDragLookAt(delta, lk);
			updateWorldToView(azimuth, elevation, range, lk);
		} else if (dragging == DRAG_LOOKAT_Z) {
			Eigen::Vector3d lk = lookAt;
			interpretDragLookAtZ(delta, lk);
			updateWorldToView(azimuth, elevation, range, lk);
		}
	}
}

void TurntableController::endDrag(const Eigen::Vector2f& mousePt) {
	if (dragging != DRAG_NULL) {
		curPt = mousePt;
		const Eigen::Vector2f delta = curPt - startPt;

		if (dragging == DRAG_ROTATE)
			interpretDragRotate(delta, azimuth, elevation);
		else if (dragging == DRAG_LOOKAT)
			interpretDragLookAt(delta, lookAt);
		else if (dragging == DRAG_LOOKAT_Z)
			interpretDragLookAtZ(delta, lookAt);

		dragging = DRAG_NULL;
		updateWorldToView();
	}
}

void TurntableController::cancelDrag() {
	dragging = DRAG_NULL;
	updateWorldToView();
}

void TurntableController::scrollRange(int wheelTicks) {
	const double rate = 1.0/480.0; // FIXME: magic scaling constant? (note: 120 * 4; 120 is the size of a normal scroll wheel "click")
	range *= std::pow(1.5, double(-wheelTicks) * rate);
	updateWorldToView();
}

void TurntableController::setView(double azimuth, double elevation, double range, const Eigen::Vector3d& lookAt) {
	assert(dragging == DRAG_NULL);

	this->azimuth = azimuth;
	this->elevation = elevation;
	this->range = range;
	this->lookAt = lookAt;

	updateWorldToView();
}

void TurntableController::updateWorldToView() {
	updateWorldToView(azimuth, elevation, range, lookAt);
}

void TurntableController::updateWorldToView(double az, double el, double rg, const Eigen::Vector3d& pivot) {
	viewRotation = calculateRotation(az, el);
	worldToView =
		// push points away from the camera by 'range' units
		Eigen::Translation3d(0.0, 0.0, rg) *
		// rotate the view
		Eigen::Isometry3d(viewRotation) *
		// put the look-at point at the origin
		Eigen::Translation3d(-pivot);
}

inline Eigen::Quaterniond TurntableController::calculateRotation(double az, double el) const {
	return
		Eigen::Quaterniond(Eigen::AngleAxisd(-el, Eigen::Vector3d::UnitX())) *
		Eigen::Quaterniond(Eigen::AngleAxisd(az, Eigen::Vector3d::UnitY()));
}

inline void TurntableController::interpretDragRotate(
		const Eigen::Vector2f& delta, double& az, double& el) const {

	az += delta(0) * -5.0; // FIXME: magic scaling factor
	while (az <= -M_PI)
		az += 2.0*M_PI;
	while (az > M_PI)
		az -= 2.0*M_PI;

	const double MIN_ELEVATION = -M_PI / 4.0; // -45 degrees
	const double MAX_ELEVATION = M_PI / 2.0;  //  90 degrees

	el += delta(1) * -5.0; // FIXME: magic scaling factor???
	el = std::min(std::max(el, MIN_ELEVATION), MAX_ELEVATION);
}

inline void TurntableController::interpretDragLookAt(
		const Eigen::Vector2f& delta, Eigen::Vector3d& pivot) const {

	const Eigen::Quaterniond orient = calculateRotation(azimuth, elevation).inverse();
	pivot -= orient * Eigen::Vector3d(delta(0)*range, delta(1)*range, 0.0);
}

inline void TurntableController::interpretDragLookAtZ(
		const Eigen::Vector2f& delta, Eigen::Vector3d& pivot) const {

	const Eigen::Quaterniond orient = calculateRotation(azimuth, elevation).inverse();
	pivot -= orient * Eigen::Vector3d(delta(0)*range, 0.0, delta(1)*range);
}
